#ifndef ZHUANGYUAN_ROBOT_CONTROL_H
#define ZHUANGYUAN_ROBOT_CONTROL_H

#include <stdio.h>
#include <iostream> 
#include "AeroRobotControl.h"
#include "optoforce_data.h"
#include "end_force_control.h"
#include <Eigen/Eigen>
//#include other .h 

using namespace std;
using namespace AeroRobot;
using namespace Eigen;

class zhuangyuan_robot_control: public AeroRobotControl, public optoforce_data, public end_force_control
{
public:	
	//构造和析构函数	
	zhuangyuan_robot_control(const char * IP, const string &address, double sixth_joint_to_force_sensor);
	/**
	 * @param param 说明
	**/
	~zhuangyuan_robot_control(); 

	//public members here;
	//including variables and functions you want to call in scripts;
	double adjust_step = 0.001;
	/**
	 * @brief xy平面内每次调整的距离，单位m
	**/
	
	double adjust_velocity  = 0.02;
	/**
	 * @brief xy平面内的调整速度，单位m/s
	**/
	
	int end_move_z(double step_depth, double step_velocity);
	/**
	 * @brief 机械臂末端沿z轴（末端参考系）方向移动
	 * @param step_depth 每次前进的距离，若为正，则沿z+方向移动，负则沿z-方向移动，单位m
	 * @param step_velocity 前进速度，单位m/s
	 * @return 成功，返回1
	**/
	
	int jinyuan(double force_limit, double step_depth, double step_velocity, double z_force_limit);
	/**
	 * @brief 进源，从视觉确定的位置开始
	 * @param force_limit xy平面内的力阈值，单位N
	 * @param step_depth 每次前进的距离，单位m
	 * @param step_velocity 前进速度，单位m/s
	 * @param z_force_limit z轴力阈值，超过这个值说明进孔完成，单位N
	 * @return 成功，返回1
	**/
	
	int ningjinyuan(double step_angle, double joint_velocity, double xy_force_limit, double * z_force_range, double z_torque_limit);
	/**
	 * @brief 拧进源，从进源成功开始
	 * @param step_angle 每一步拧的角度，单位rad
	 * @param joint_velocity 拧的速度，单位rad/s
	 * @param xy_force_limit xy平面的力阈值，超过这个阈值会调整xy平面内的位置，单位N
	 * @param z_force_range double[2], z轴力的最小值和最大值，单位N
	 * @param z_torque_limit 预紧力矩，达到这个力矩停止，单位Nm
	 * @return 成功，返回1
	**/
	
	int zhuangyuan();
	
protected:
	//protected members here;
	//normally, most of variables and inside functions are here;
	
	double force_data[6] = {0, 0, 0, 0, 0, 0};

private:
	//private members here;
};

#endif
